#include "WProgram.h"
// This sketch reads IMU data from the SparkFun 5DOF IMU found at:
// http://www.sparkfun.com/commerce/product_info.php?products_id=741
// The AREF pin is connected to 3.3V to better use the 10 bit A/D

// Debug data 
#define BAUD 38400  

// Define pin assignments
#define LEDPIN 13
#define XACCELPIN 2 // I miswired my board, change order based on your setup
#define YACCELPIN 0
#define ZACCELPIN 1
#define XRATEPIN 4
#define YRATEPIN 3

// Define sensor measurements
void setup();
void loop();
int Xaccel = 0;
int Yaccel = 0;
int Zaccel = 0;
int Xrate = 0;
int Yrate = 0;

unsigned long previousTime = 0;
unsigned long deltaTime = 0;

void setup() {
  digitalWrite(LEDPIN, HIGH);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  Serial.begin(BAUD);
}

void loop() {
  previousTime = millis();

  // Read sensor measurements
  Xaccel = analogRead(XACCELPIN);
  Yaccel = analogRead(YACCELPIN);
  Zaccel = analogRead(ZACCELPIN);
  Xrate = analogRead(XRATEPIN);
  Yrate = analogRead(YRATEPIN);
  
  deltaTime = millis() - previousTime;

  digitalWrite(LEDPIN, HIGH);
  Serial.print(deltaTime);
  Serial.print(",");
  Serial.print(Xaccel);
  Serial.print(",");
  Serial.print(Yaccel);
  Serial.print(",");
  Serial.print(Zaccel);
  Serial.print(",");
  Serial.print(Xrate);
  Serial.print(",");
  Serial.println(Yrate);
  digitalWrite(LEDPIN, LOW);  
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

